#!/usr/bin/python3
# -*- coding: utf-8 -*-
# 智能小车大脑（决策者）

import _thread
import time
import move_base as MOVER
import instinct as INSTINCT
import sense as SENSE

#运动指令名称
INSTRUCT_KEY = "MoveInstruct"
#运动指令值编号
#继续保持当前运动状态
MOVE_INSTRUCT_GOON = 0

#通过超声波测距出的结果：前方180度内的障碍物距离(从里面读当前外部障碍物距离)
ObstacleDistance = None
#指令池(往里面传入运动指令，让move_base改变运动状态)
InstructMap = None

#一级危险距离
DANGER_DIST1 = 60
#二级危险距离
DANGER_DIST2 = 10
#三级危险距离
DANGER_DIST3 = 5



def init():
    global ObstacleDistance, InstructMap
    #初始化超声波测距传感器
    ObstacleDistance = [0 for i in range(19)]
    SENSE.init(ObstacleDistance)
    #开始扫描测距
    _thread.start_new_thread(ranging_UT, ())
    #初始化基础运动
    InstructMap = {INSTRUCT_KEY:MOVE_INSTRUCT_GOON}
    MOVER.init(InstructMap)
 
def clean():
    MOVER.clean()
    SENSE.clean()
    
#超声波测距
def ranging_UT():
    while(True):
        SENSE.scanArount()

        
#循环判断是否有危险
def scanDangerMove():
    while(True):
        print(ObstacleDistance)
        fx = checkSafe()
        if (-1 == fx):#向左转
            moveIndex = 3
            moveTimes = 0.75
        elif (1 == fx):#向右转
            moveIndex = 2
            moveTimes = 0.75
        elif 0 == fx:#向前
            moveIndex = 1
            moveTimes = 3
        elif 2 == fx:#左后转弯
            moveIndex = 6
            moveTimes = 0.75
        elif 3 == fx:#右后转弯
            moveIndex = 5
            moveTimes = 0.75
        else:
            moveIndex = 7
            moveTimes = 0.8
        move(moveIndex, moveTimes, 2)
        if 0 != fx:
            time.sleep(2)

#正常随机运动
def commonMove():
    while(True):
        moveIndex = INSTINCT.randomove()
        moveTimes = INSTINCT.randomTime()
        move(moveIndex, moveTimes, 1)
        time.sleep(moveTimes)
        
#运动
def move(moveIndex, moveTimes, instinctFrom):
    print("instinctFrom=" + str(instinctFrom) + ", moveIndex=" + str(moveIndex) + ", moveTimes=" + str(moveTimes))
    
    if 1 == moveIndex:
        MOVER.forward(moveTimes)
    elif 2 == moveIndex:
        MOVER.turnRight(moveTimes)
    elif 3 == moveIndex:
        MOVER.turnLeft(moveTimes)
    elif 4 == moveIndex:
        MOVER.back(moveTimes)
    elif 5 == moveIndex:
        MOVER.backRight(moveTimes)
    elif 6 == moveIndex:
        MOVER.backLeft(moveTimes)
    elif 7 == moveIndex:
        MOVER.aroundCW(moveTimes)
    elif 8 == moveIndex:
        MOVER.aroundCCW(moveTimes)
    else:
        MOVER.noMove(moveTimes)

#判断左右是否危险
def checkLeftAndRight():
    leftDange = False
    rightDange = False
    for i in ObstacleDistance[0:10]:
        if i < 100:
            leftDange = True
            break
    for i in ObstacleDistance[10:19]:
        if i < 100:
            rightDange = True
            break
    return leftDange, rightDange

#判断左右哪里更安全 左-1  中0 右1 (加起来)
def checkSafe():
    res = 0
    #单方向 前、左、右
    if ObstacleDistance[9] > DANGER_DIST1:
        if ObstacleDistance[0] > DANGER_DIST1:
            if ObstacleDistance[18] > DANGER_DIST1:
                return 0
            else:
                return checkSafeByAverage(1)
        else:
            if ObstacleDistance[18] > DANGER_DIST1:
                return checkSafeByAverage(0)
            else:
                return -1
    elif ObstacleDistance[9] > 10 and ObstacleDistance[9] <= DANGER_DIST1:
        if ObstacleDistance[0] > DANGER_DIST1:
            if ObstacleDistance[18] > DANGER_DIST1:
                return checkSafeByAverage(-1)
            else:
                return 0
        else:
            if ObstacleDistance[18] > DANGER_DIST1:
                return 1
            else:
                return checkSafeByAverage()
    elif ObstacleDistance[9] > 5 and ObstacleDistance[9] <= 10:
        if checkSafeByAverage(0) == -1:
            return 2
        else: 
            return 3
    else:
        return 4
        
def checkSafeByAverage(excludeDir=2):
    sumLeft = 0
    sumMiddle = 0
    sumRight = 0
    
    for i in ObstacleDistance[0:6]:
        sumLeft += i
    for i in ObstacleDistance[6:13]:
        sumMiddle += i
    for i in ObstacleDistance[13:19]:
        sumRight += i
        
    if 2 == excludeDir:    
        if sumMiddle/7 >= sumLeft/6 and sumMiddle/7 >= sumRight/6:
            return 0
        elif sumLeft/6 >= sumMiddle/7 and sumLeft/6 >= sumRight/6:
            return -1
        else:
            return 1
    elif -1 == excludeDir:#中/右比较
        if sumMiddle/7 >= sumRight/6:
            return 0
        else:
            return 1
    elif 0 == excludeDir:#左右比较
        if sumLeft/6 >= sumRight/6:
            return -1
        else:
            return 1
    elif 1 == excludeDir:#左中比较
        if sumLeft/6 >= sumMiddle/7:
            return -1
        else:
            return 0
    else:
        return 0
        
            
#-------------TEST----------------
if __name__ == "__main__":
    init()
    try:
        _thread.start_new_thread(commonMove, ())
        _thread.start_new_thread(scanDangerMove, ())
        time.sleep(600)
    except KeyboardInterrupt:
        clean()
        print("捕捉异常后清理资源完成")
    else:
        clean()
        print("正常测试结束")

    